
#include <fcntl.h>
#include <unistd.h>

#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include <sys/stat.h>
#include <sys/mman.h>

#include <libv4l2.h>
#include <libv4lconvert.h>

#include "../../util/list.h"
#include "../../util/mem.h"
#include "../../util/msg.h"

#include "v4l.h"
#include "v4l2_driver.h"


#define N_MAX_CAM 15

//==============================================================================

int v4l_get_input(struct v4l_driver * v4ldrv, struct v4l2_input * input) {
	return v4l2_driver_get_input(v4ldrv->fd, input);
}

int v4l_get_standard(struct v4l_driver * v4ldrv, struct v4l2_standard * standard) {
	return v4l2_driver_get_standard(v4ldrv->fd, standard);
}

int v4l_get_format(struct v4l_driver * v4ldrv, struct v4l2_format * format) {
	return v4l2_driver_get_format(v4ldrv->fd, format, V4L2_BUF_TYPE_VIDEO_CAPTURE);
}

int v4l_get_format_description(struct v4l_driver * v4ldrv, struct v4l2_fmtdesc * fmtdesc) {
	return v4l2_driver_get_format_description(v4ldrv->fd, fmtdesc, V4L2_BUF_TYPE_VIDEO_CAPTURE);
}

int v4l_get_control(struct v4l_driver * v4ldrv, struct v4l2_control * ctrl, int ctrl_id) {
	return v4l2_driver_get_control(v4ldrv->fd, ctrl, ctrl_id);
}

//==============================================================================

int v4l_set_input(struct v4l_driver * v4ldrv, int index) {
	return v4l2_driver_set_input(v4ldrv->fd, &index);
}

int v4l_set_standard(struct v4l_driver * v4ldrv, v4l2_std_id id) {
	return v4l2_driver_set_standard(v4ldrv->fd, &id);
}

int v4l_set_pixel_format(struct v4l_driver * v4ldrv, int pixel_format) {
	struct v4l2_format format;

	v4l2_driver_get_format(v4ldrv->fd, &format, V4L2_BUF_TYPE_VIDEO_CAPTURE);

	format.fmt.pix.pixelformat = pixel_format;
	return v4l2_driver_set_format(v4ldrv->fd, &format);
}

int v4l_set_frame_size(struct v4l_driver * v4ldrv, int width, int height) {
	struct v4l2_format format;

	v4l2_driver_get_format(v4ldrv->fd, &format, V4L2_BUF_TYPE_VIDEO_CAPTURE);

	format.fmt.pix.width = width;
	format.fmt.pix.height = height;
	return v4l2_driver_set_format(v4ldrv->fd, &format);
}

int v4l_set_control(struct v4l_driver * v4ldrv, int ctrl_id, int value) {
	struct v4l2_control ctrl;

	ctrl.id = ctrl_id;
	ctrl.value = value;
	return v4l2_driver_set_control(v4ldrv->fd, &ctrl);
}

//==============================================================================

int v4l_enum_devices(char ** dev, int init, int max_dev) {

	struct stat st;
	static int __dev_count = 0;

	int len;
	char name[64];

	if (init) __dev_count = 0;
	if (__dev_count >= max_dev) return 0;

	snprintf(name, 64, "/dev/video%d", __dev_count);
	__dev_count++;

	if (stat(name, &st) != 0) return 0;

	len = strlen(name);
	(*dev) = mem_allocn(len + 1, char);
	strcpy(*dev, name);
	(*dev)[len] = '\0';

	return 1;
}

int v4l_enum_input(struct v4l_driver * v4ldrv, struct v4l2_input * input, int init, int index) {
	return v4l2_driver_enum_input(v4ldrv->fd, input, init, index);
}

int v4l_enum_standard(struct v4l_driver * v4ldrv, struct v4l2_standard * standard, int init, int index) {
	return v4l2_driver_enum_standard(v4ldrv->fd, standard, init, index);
}

int v4l_enum_format_desc(struct v4l_driver * v4ldrv, struct v4l2_fmtdesc * fmtdesc, int init, int index) {
	return v4l2_driver_enum_format_description(v4ldrv->fd, fmtdesc, init, index, V4L2_BUF_TYPE_VIDEO_CAPTURE);
}

int v4l_enum_frame_size(struct v4l_driver * v4ldrv, struct v4l2_frmsizeenum * frmsize, int pixel_format, int init, int index) {
	return v4l2_driver_enum_frame_size(v4ldrv->fd, frmsize, init, pixel_format, index);
}

int v4l_enum_control(struct v4l_driver * v4ldrv, struct v4l2_queryctrl * qctrl, int init) {
	return v4l2_driver_enum_control(v4ldrv->fd, qctrl, init);
}

int v4l_enum_control_user(struct v4l_driver * v4ldrv, struct v4l2_queryctrl * qctrl, int init) {
	return v4l2_driver_enum_control_user(v4ldrv->fd, qctrl, init);
}

int v4l_enum_control_private(struct v4l_driver * v4ldrv, struct v4l2_queryctrl * qctrl, int init) {
	return v4l2_driver_enum_control_private(v4ldrv->fd, qctrl, init);
}

int v4l_enum_control_menu(struct v4l_driver * v4ldrv, struct v4l2_querymenu * qmenu, int ctrl_id, int index) {
	return v4l2_driver_enum_control_menu(v4ldrv->fd, qmenu, ctrl_id, index);
}
//==============================================================================

List * v4l_get_device_list(void) {

	List * L = list_new();
	if (!L) {
		//_msg( MSG_ERROR, "Can't allocate list memory!\n" );
		//exit(EXIT_FAILURE);
		return 0x0;
	}

	struct stat st;
	char * name;
	int n = 0;

	while (n < N_MAX_CAM) {
		name = (char *) calloc(DEVICE_NAME_MAX_LENGTH, sizeof (char));
		snprintf(name, DEVICE_NAME_MAX_LENGTH, "/dev/video%d", n);

		if (stat(name, &st) != 0) {
			free(name);
			//break;
		} else {
			list_insert(L, name);
		}
		n++;
	}
	return L;
}

//==============================================================================

static struct v4l_driver v4l_open(const char * device, int fmt_emulation) {

	struct v4l_driver v4ldrv;
	memset(&v4ldrv, 0, sizeof (v4ldrv));

	int fd;
	fd = open(device, O_RDWR | O_NONBLOCK);
	if (fd < 0) {
		__error("Cannot open %s\n", device);
	}

	//struct v4l2_capability cap;

	if (!v4l2_driver_query_capability(fd, &v4ldrv.vcap)) {
		__error("%s is not a V4L2 device\n", device);
	}

	v4ldrv.fd = fd;
	v4ldrv.fmt_emulation = fmt_emulation;

	if (fmt_emulation) {
		int fd2 = v4l2_fd_open(fd, V4L2_ENABLE_ENUM_FMT_EMULATION);
		if (fd2 < 0) {
			v4ldrv.fmt_emulation = 0;
			__warning("Cannot use libv4l2 wrapper for %s \n", device);
		}
	}

	v4l2_driver_get_input(fd, &v4ldrv.input);
	//v4l2_driver_get_standard(fd, &v4ldrv.standard);
	//v4l2_driver_get_format(fd, &v4ldrv.format, V4L2_BUF_TYPE_VIDEO_CAPTURE);
	//v4l2_driver_get_format(fd, &v4ldrv.format, V4L2_BUF_TYPE_VIDEO_CAPTURE);



	v4ldrv.v4lconv = v4lconvert_create(v4ldrv.fd);

	return v4ldrv;
}

struct v4l_driver v4l_open_default(const char * device) {

	return v4l_open(device, 1);

}

int v4l_close(struct v4l_driver * v4ldrv) {


	v4lconvert_destroy(v4ldrv->v4lconv);

	if (v4ldrv->fmt_emulation) return v4l2_close(v4ldrv->fd);

	return close(v4ldrv->fd);
}

int v4l_start(struct v4l_driver * v4ldrv) {

	int r;
	int fd = v4ldrv->fd;

	// = Get current format ====================================================
	struct v4l2_format fmt;
	//fmt = v4ldrv->format_src;
	r = v4l2_driver_get_format(fd, &fmt, V4L2_BUF_TYPE_VIDEO_CAPTURE);
	if (v4ldrv->fmt_emulation && fmt.fmt.pix.pixelformat != V4L2_PIX_FMT_RGB24) {

		struct v4l2_fract interval;
		int interval_ok = v4l2_driver_get_interval(fd, &interval);
		fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24;

		r = v4l2_driver_set_format(fd, &fmt);
		r = v4l2_driver_get_format(fd, &fmt, V4L2_BUF_TYPE_VIDEO_CAPTURE);

		if (interval_ok) {
			v4l2_driver_set_interval(fd, &interval);
		}
	}

	v4ldrv->width = fmt.fmt.pix.width;
	v4ldrv->height = fmt.fmt.pix.height;
	v4ldrv->buffer_raw = mem_allocn(fmt.fmt.pix.sizeimage, unsigned char);

	v4ldrv->format_src = fmt;

	// = Request buffers by mmap ===============================================
	struct v4l2_requestbuffers reqbuf;
	r = v4l2_driver_cap_request_buffers_mmap(fd, &reqbuf, 2);
	if (!r) __error("Can't request mmap buffers.\n");


	if (reqbuf.count < 2) {
		v4l2_driver_cap_request_buffers_mmap(fd, &reqbuf, 0);
		__error("Insufficient buffer memory.\n");
	}

	// = Alloc buffers =========================================================
	v4ldrv->v4lbuffers.qtd = reqbuf.count;
	v4ldrv->v4lbuffers.buffers = (struct buffer *) calloc(reqbuf.count, sizeof (struct buffer));

	if (!v4ldrv->v4lbuffers.buffers) {
		return 0;
	}

	// = Memory map: mmap ======================================================
	r = v4l2_driver_mmap(fd, &v4ldrv->v4lbuffers, &reqbuf);
	if (!r) __error("Can't mmap.\n");


	// = Enqueue all buffers ===================================================
	int i;
	for (i = 0; i < v4ldrv->v4lbuffers.qtd; i++) {
		r = v4l2_driver_enqueue_buff_x(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, i);
		if (!r) __warning("Can't enqueue buffer %d.\n", i);
	}

	// = Streamon ==============================================================
	r = v4l2_driver_streamon(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE);
	if (!r) __warning("Can't streamon\n");

	return 1;
}

void * v4l_grab_raw_old2(struct v4l_driver * v4ldrv) {
	struct v4l2_buffer buf;
	int r;
	int again = 1;
	int fd;

	fd = v4ldrv->fd;

	while (again) {
		r = v4l2_driver_cap_dequeue_buffer_mmap(fd, &buf, &again);
		if (!r) __warning("Can't dequeue buffers.\n");
	}

	r = v4l2_driver_enqueue_buffer(fd, &buf);
	if (!r) __warning("Can't enqueue buffers.\n");

	return v4ldrv->v4lbuffers.buffers[buf.index].start;

}

void * old_v4l_grab_raw(struct v4l_driver * v4ldrv) {

	int fd;
	int r;

	fd = v4ldrv->fd;

	struct v4l2_buffer vbuf;
	memset(&vbuf, 0, sizeof (vbuf));

	/* dequeue buffer */
	vbuf.index = v4ldrv->dq_i;
	vbuf.field = V4L2_FIELD_ANY;
	vbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	vbuf.memory = V4L2_MEMORY_MMAP;

	do {
		r = v4l2_driver_dequeue_buffer(fd, &vbuf);
	} while (!r && r < 100);
	if (r >= 100) {
		__warning("Can't dequeue buffer.\n");
	}

	v4ldrv->bytesused = vbuf.bytesused;
	v4ldrv->buf_pointer = v4ldrv->v4lbuffers.buffers[v4ldrv->dq_i].start;

	/* actualize next dequeue index */
	v4ldrv->dq_i++;
	if (v4ldrv->dq_i >= v4ldrv->v4lbuffers.qtd) v4ldrv->dq_i = 0;

	/* enqueue next buffer */
	memset(&vbuf, 0, sizeof (vbuf));
	vbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	vbuf.memory = V4L2_MEMORY_MMAP;
	vbuf.index = v4ldrv->eq_i;

	r = v4l2_driver_enqueue_buffer(fd, &vbuf);
	if (!r) __warning("Can't enqueue buffer.\n");

	/* actualize next enqueue index */
	v4ldrv->eq_i++;
	if (v4ldrv->eq_i >= v4ldrv->v4lbuffers.qtd) v4ldrv->eq_i = 0;

	/* return the current dequeued buffer */
	return v4ldrv->buf_pointer;
}

void * v4l_grab_raw(struct v4l_driver * v4ldrv) {

	int fd;
	int r;

	fd = v4ldrv->fd;

	struct v4l2_buffer vbuf;
	memset(&vbuf, 0, sizeof (vbuf));

	/* dequeue buffer */
	vbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	vbuf.memory = V4L2_MEMORY_MMAP;

	// =========================================================================
	int count = 0;
	int again = 0;

	do {
		r = v4l2_driver_cap_dequeue_buffer_mmap(fd, &vbuf, &again);
		if (count > 100) __error("Cannot dequeue buffer %d by mmap.\n", vbuf.index);
		count++;
	} while (again);
	// =========================================================================

	v4ldrv->bytesused = vbuf.bytesused;
	v4ldrv->buf_pointer = v4ldrv->v4lbuffers.buffers[vbuf.index].start;
	memcpy(v4ldrv->buffer_raw, v4ldrv->buf_pointer, v4ldrv->bytesused);

	// = enqueue next buffer ===================================================
	r = v4l2_driver_enqueue_buffer(fd, &vbuf);
	if (!r) {
		//__warning("Can't enqueue buffer.\n");
		r = 0;
	}

	// = return the current dequeued buffer pointer
	return v4ldrv->buf_pointer;
}

void * v4l_grab_gray(struct v4l_driver * v4ldrv, char * gray) {

	void * buffer;
	struct v4l2_format fmtdest;

	fmtdest.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	fmtdest.fmt.pix.width = v4ldrv->width;
	fmtdest.fmt.pix.height = v4ldrv->height;
	fmtdest.fmt.pix.pixelformat = V4L2_PIX_FMT_GREY;

	buffer = v4l_grab_raw(v4ldrv);

	if (!buffer) return NULL;

	int size = v4ldrv->width * v4ldrv->height;

	v4lconvert_convert(v4ldrv->v4lconv, &v4ldrv->format_src,
			&fmtdest,
			buffer,
			v4ldrv->bytesused,
			(unsigned char *) gray,
			size);
	return gray;
}

void * v4l_grab_rgb(struct v4l_driver * v4ldrv, char * rgb) {

	void * buffer;
	struct v4l2_format fmtdest;

	fmtdest = v4ldrv->format_src;
	fmtdest.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24;

	buffer = v4l_grab_raw(v4ldrv);

	//char * data = mem_allocn(v4ldrv->bytesused, char);
	//memcpy(v4ldrv->buffer, v4ldrv->buffer_raw, v4ldrv->bytesused);

	int size = v4ldrv->width * v4ldrv->height * 3;

	v4lconvert_convert(v4ldrv->v4lconv,
			&v4ldrv->format_src,
			&fmtdest,
			v4ldrv->buffer_raw,
			v4ldrv->bytesused,
			(unsigned char *) rgb,
			size);
	return rgb;
}

int v4l_stop(struct v4l_driver * v4ldrv) {

	int fd;
	int r;

	fd = v4ldrv->fd;

	if (v4ldrv->v4lbuffers.buffers == NULL) return 1;

	// = streamoff =============================================================
	r = v4l2_driver_streamoff(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE);

	// = unmmap ================================================================
	v4l2_driver_munmap(&v4ldrv->v4lbuffers);

	// = free all buffers ======================================================
	struct v4l2_requestbuffers reqbuf;

	r = v4l2_driver_cap_request_buffers_mmap(fd, &reqbuf, 1);
	if (!r) __warning("Can't request 1 mmap buffers.\n");
	r = v4l2_driver_cap_request_buffers_mmap(fd, &reqbuf, 0);
	if (!r) __warning("Can't request 0 mmap buffers.\n");

	mem_free(v4ldrv->buffer_raw);
	mem_free(v4ldrv->v4lbuffers.buffers);
	v4ldrv->v4lbuffers.qtd = 0;
	//	memset(v4ldrv, 0, sizeof (*v4ldrv));



	return 1;
}
